#include "Serial32.h"

using namespace std;

//发送数据缓冲
#define TXBUFNUM 100
static myTxFrame txFrameBuf[TXBUFNUM];
static uint8_t txBufWAddr = 0;
static uint8_t txBufRAddr = 0;



/*左右编码器值*/
int16_t L_Encoder, R_Encoder;


//高度值
float Alt;


/**
 * 串口初始化
 */
Serial32::Serial32(std::string Port, uint32_t Baudrate,  uint32_t timeout, const std::string & node_name) : Node(node_name)
{

    try {
        ser.setPort(Port);    
        ser.setBaudrate(Baudrate);
        _timeout = serial::Timeout::simpleTimeout(timeout);
		

        ser.open();
		ser.setFlowcontrol(serial::flowcontrol_none); 
        ser.flushInput();
        RCLCPP_INFO(rclcpp::get_logger("Serial32"), "Serial Port opened");
    }
    catch (serial::IOException& e) {
        RCLCPP_ERROR(rclcpp::get_logger("Serial32"), "Unable to open port");
    }
    if (ser.isOpen()) {
        ser.flushInput();
        RCLCPP_INFO(rclcpp::get_logger("Serial32"), "Serial Port opened successfully");
    }

	timer_ = this->create_wall_timer(
        1ms, 
        std::bind(&Serial32::timer_callback, this)
    );

	cmd_vel_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
		"/cmd_vel", 10, std::bind(&Serial32::cmd_vel_callback, this, std::placeholders::_1));
	alt_pub = this->create_publisher<sensor_msgs::msg::Range>("/height", 10);



}

/**
 * 析构函数
 */
Serial32::~Serial32()
{
    ser.close();
}


/**
 * 接收stm32发来的一个字节，先高位后低位
 * 帧头：0xAA 
 * addr:0xFF
*/
bool Serial32::Ser_ReceiveOneByte(uint8_t Byte)
{
    
	static uint16_t _recv_cnt = 0;
	static uint16_t _data_cnt = 0;
	static uint8_t rxstate = 0;
	//RCLCPP_INFO(this->get_logger(), "%x", Byte);

	// static uint64_t lastRxTime = 0;  // 记录最后接收时间（需初始化为0）
	// //卡住超过20ms自动清零
	// if (uint64_t(get_clock()->now().) - lastRxTime >= 12) {
	// 	rxstate = 0;
	// 	_data_cnt = 0;
	// 	_recv_cnt = 0;
		
	// }

	if (_recv_cnt >= 15) {
		rxstate = 0;
		_data_cnt = 0;
		_recv_cnt = 0;
	}

	/*帧头*/
	if (rxstate == 0 && Byte == 0xAA) {
		_recv_cnt = 0;
		RxFrame.rawBytes[_recv_cnt++] = Byte;
		rxstate++;
		
	}
	/*地址*/
	else if (rxstate == 1) {
		RxFrame.rawBytes[_recv_cnt++] = Byte;
		rxstate++;
	}
	/*ID*/
	else if (rxstate == 2) {
		RxFrame.rawBytes[_recv_cnt++] = Byte;
		rxstate++;
	}
	/*数据长度*/
	else if (rxstate == 3) {
		RxFrame.rawBytes[_recv_cnt++] = Byte;
		RxFrame.frame.dataLen = Byte;
		rxstate++;
		_data_cnt = 0;
	}
	/*数据*/
	else if (rxstate == 4) {
		RxFrame.rawBytes[_recv_cnt++] = Byte;
		_data_cnt++;
		if (_data_cnt >= RxFrame.frame.dataLen) {
			rxstate++;
		}
	}
	/*SC1*/
	else if (rxstate == 5) {
		RxFrame.frame.sc1 = Byte;
		rxstate++;
	}
	/*SC2*/
	else if (rxstate == 6) {
		RxFrame.frame.sc2 = Byte;
		rxstate = 0;
		return Rx_Frame_Anl();

	}
	else {
		rxstate = 0;
        return false;
	}
    return false;
}

/**
 * 解析数据包
 */
bool Serial32::Rx_Frame_Anl()
{
    /*校验和*/
	uint8_t check_sum1 = 0;
	uint8_t check_sum2 = 0;
	for (uint16_t i = 0; i < (RxFrame.frame.dataLen + 4); i++) {
		check_sum1 += RxFrame.rawBytes[i];
		check_sum2 += check_sum1;
		//ROS_INFO("%2x", RxFrame.rawBytes[i]);
	}
	// ROS_INFO("ch1:%2x,ch1_:%2x", check_sum1, RxFrame.frame.sc1);
	// ROS_INFO("ch2:%2x,ch2_:%2x", check_sum2, RxFrame.frame.sc2);
	if (check_sum1 != RxFrame.frame.sc1 || check_sum2 != RxFrame.frame.sc2) {
		RCLCPP_WARN(rclcpp::get_logger("Serial32"), "Checksum Error");
		return false;
	}
	
	switch (RxFrame.frame.ID)
	{

	case 0x01:
	{	
		// L_Encoder= (int16_t)*((int16_t*)(RxFrame.frame.dataBuf + 0));
		// R_Encoder = (int16_t)*((int16_t*)(RxFrame.frame.dataBuf + 2));
		// 使用逐字节方式手动重建16位整数
		// L_Encoder = (int16_t)(RxFrame.frame.dataBuf[1] | (RxFrame.frame.dataBuf[0] << 8));
		// R_Encoder = (int16_t)(RxFrame.frame.dataBuf[3] | (RxFrame.frame.dataBuf[2] << 8));

		//SROS_INFO("OK");
        // return true;
        // break;
	}
	
	case 0x05:
	{
		Alt = (float)*((int16_t*)(RxFrame.frame.dataBuf+0)) / 100;
		return true;
		break;
	}


	default:
        return false;
		break;
	}
    return false;

}

/**
 * 读取串口
 */
bool Serial32::Read_SerialData(void)
{
    int _cnt = ser.available();
    if (_cnt != 0) {
        ser.read(read_buf, _cnt);
        for (int i = 0; i < _cnt; i++) {
            if (Ser_ReceiveOneByte(read_buf[i]))
                return true;
        }
    }
    return false;
}

void Serial32::SendSpeedTo32(int16_t Lspeed, int16_t Rspeed)
{
	TxFrame.frame.head = 0xAA;
	TxFrame.frame.addr = 0xFF;
	TxFrame.frame.ID = 0x01;
	TxFrame.frame.dataLen = 4;
	TxFrame.frame.dataBuf[0] = (uint8_t)(Lspeed >> 8);   
	TxFrame.frame.dataBuf[1] = (uint8_t)(Lspeed & 0xFF); 
	TxFrame.frame.dataBuf[2] = (uint8_t)(Rspeed >> 8);  
	TxFrame.frame.dataBuf[3] = (uint8_t)(Rspeed & 0xFF); 


	uint8_t sc1 = 0, sc2 = 0;
	for (int i = 0; i < (4 + TxFrame.frame.dataLen); i++) {
		sc1 += TxFrame.rawBytes[i];
		sc2 += sc1;
	}
	TxFrame.rawBytes[TxFrame.frame.dataLen + 4] = sc1;
	TxFrame.rawBytes[TxFrame.frame.dataLen + 5] = sc2;

	ser.write(TxFrame.rawBytes, TxFrame.frame.dataLen + 6);


}


void Serial32::pub_alt(void)
{
	//发布Alt
	if (Alt >= 0) {
		sensor_msgs::msg::Range message;
		message.min_range = 0;
		message.max_range = 99999;
		message.range = Alt;
		alt_pub->publish(message);
	}
}


//串口发送数据
void Serial32::Ser_Send(uint8_t linktype, uint8_t *buf, uint16_t len)
{
  this->ser.write(buf,len);
}


void Serial32::AddTxFrame(uint8_t linkType, myRxFrame *pFrame)
{
  if(txFrameBuf[txBufWAddr].cmd._isUsed)
    return;
  txFrameBuf[txBufWAddr].cmd._isUsed = 1;
  txFrameBuf[txBufWAddr].cmd._linkType = linkType;
  for(uint16_t i = 0; i < (pFrame->frame.dataLen + 6); i++)
    txFrameBuf[txBufWAddr].data.rawBytes[i] = pFrame->rawBytes[i];
  
  txFrameBuf[txBufWAddr].cmd._isUsed = 2;
  
  txBufWAddr++;
  if(txBufWAddr >= TXBUFNUM)
    txBufWAddr = 0;
}


void Serial32::SendCheck(void)
{
  //如果发送缓冲区有需要发送的数据，执行发送
  for(uint8_t i=0; i<TXBUFNUM; i++)
  {
    if(txFrameBuf[txBufRAddr].cmd._isUsed == 2)
    {
      //数据写入完毕，调用发送
      Ser_Send(txFrameBuf[txBufRAddr].cmd._linkType, txFrameBuf[txBufRAddr].data.rawBytes, txFrameBuf[txBufRAddr].data.frame.dataLen+6);
      txFrameBuf[txBufRAddr].cmd._isUsed = 0;
    }
    txBufRAddr++;
    if(txBufRAddr >= TXBUFNUM)
    txBufRAddr = 0;
  }
}



void Serial32::SendBuf(const uint8_t ID,const uint8_t* buf, const uint8_t len)
{
  
  myRxFrame txFrame;
  uint8_t _cnt = 0;

  txFrame.frame.head = 0xAA;
  txFrame.frame.addr = 0xFF;
  txFrame.frame.ID = ID;
  for(uint8_t i = 0; i < len; i++)
  {
    txFrame.frame.dataBuf[_cnt++] = *(buf + i);
  }
  txFrame.frame.dataLen = _cnt;
  uint8_t sc1 = 0;
  uint8_t sc2 = 0;
  for(uint16_t i=0; i<(txFrame.frame.dataLen+4); i++)
  {
    sc1 += txFrame.rawBytes[i];
    sc2 += sc1;
  }
  txFrame.rawBytes[txFrame.frame.dataLen+4] = sc1;
  txFrame.rawBytes[txFrame.frame.dataLen+5] = sc2;

	AddTxFrame(0x01, &txFrame);
}
